/*
 * @Author: newsun
 * @Date: 2022-02-15 11:00:17
 * @LastEditTime: 2022-02-15 16:10:44
 * @LastEditors: newsun-HP-Pavilion-Gaming-Laptop-15-dk0xxx
 * @Description: 击打前哨站中部旋转装甲板
 * @FilePath: /Vision_bak/src/armor/include/aim_tower/HitTower.hpp
 */
#ifndef _HIT_TOWER_HPP
#define _HIT_TOWER_HPP
#include <opencv2/opencv.hpp>
#include <opencv2/highgui.hpp>
#include <algorithm>
#include "readParam.hpp"
#include "../aim_deps/AimDeps.hpp"
#include "../aim_pre/Predict.hpp"
#include "serial_com/comm.h"
#include <Eigen/Geometry>
#include <Eigen/Cholesky>
#include <Eigen/Core>



class HitTower{
public:
    HitTower(){
        this->init_time = std::chrono::system_clock::now().time_since_epoch().count() / 1e9;
    }
    ~HitTower(){ ; }
private:

    double init_time;//每次启动时的初始时间,单位为秒
    double spin_period = 2.5;//装甲板旋转周期为2.5s
    double time_eps = 0.008;//判断浮点数时间接近的绝对误差
    double air_k;
//存储的对应前哨站坐标(世界坐标系下的)以及对应的时间戳
    std::deque<float> history_x;
    std::deque<float> history_z;
    std::deque<double> history_t;

public:
    void reset();
    void initWithROS(ros::NodeHandle& nh);
    void hit(const cv::Point3f& t_cam,const serial_com::comm& msg, Eigen::Vector3d& cam_p);//装甲板击打主模块，仅仅输入前哨站装甲板的坐标
    
private:
    
    double getCurrentTime();//获取从程序开始运行到目前为止的时间
    double calPreditTime(float dist,float bullet_v, float pitch,float air_k);//计算预测的时间
};

#endif